#include <gtest/gtest.h>
#include <gflags/gflags.h>
#include <glog/logging.h>

#include <pcl/io/pcd_io.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf/transform_datatypes.h>

#include <geometry_msgs/Quaternion.h>
#include <nav_msgs/Odometry.h>

#include "utils/TicToc.h"
#include "utils/Twist.h"
#include "utils/common_ros.h"

typedef pcl::PointXYZI PointT;
typedef typename pcl::PointCloud<PointT> PointCloud;
typedef typename pcl::PointCloud<PointT>::Ptr PointCloudPtr;
typedef lio::Twist<float> Transform;

Transform transform_sum_;
ros::Time time_laser_cloud_corner_last_;   ///< time of current last corner cloud
ros::Time time_laser_cloud_surf_last_;     ///< time of current last surface cloud
ros::Time time_laser_full_cloud_;      ///< time of current full resolution cloud
ros::Time time_laser_odometry_; 
bool new_laser_cloud_corner_last_;  ///< flag if a new last corner cloud has been received
bool new_laser_cloud_surf_last_;    ///< flag if a new last surface cloud has been received
bool new_laser_full_cloud_;     ///< flag if a new full resolution cloud has been received
bool new_laser_odometry_; 
PointCloudPtr laser_cloud_corner_last_(new PointCloud());   ///< last corner points cloud
PointCloudPtr laser_cloud_surf_last_(new PointCloud());     ///< last surface points cloud
PointCloudPtr full_cloud_(new PointCloud());      ///< last full resolution cloud
ros::Subscriber sub_laser_cloud_corner_last_;
ros::Subscriber sub_laser_cloud_surf_last_;
ros::Subscriber sub_laser_full_cloud_;
ros::Subscriber sub_laser_odometry_;
ros::Publisher pub_compact_data_;

void LaserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr &corner_points_sharp_msg) {
  time_laser_cloud_corner_last_ = corner_points_sharp_msg->header.stamp;

  laser_cloud_corner_last_->clear();
  pcl::fromROSMsg(*corner_points_sharp_msg, *laser_cloud_corner_last_);

  new_laser_cloud_corner_last_ = true;
}

void LaserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr &corner_points_less_sharp_msg) {
  time_laser_cloud_surf_last_ = corner_points_less_sharp_msg->header.stamp;

  laser_cloud_surf_last_->clear();
  pcl::fromROSMsg(*corner_points_less_sharp_msg, *laser_cloud_surf_last_);

  new_laser_cloud_surf_last_ = true;
}

void LaserFullCloudHandler(const sensor_msgs::PointCloud2ConstPtr &full_cloud_msg) {
  time_laser_full_cloud_ = full_cloud_msg->header.stamp;

  full_cloud_->clear();
  pcl::fromROSMsg(*full_cloud_msg, *full_cloud_);

  new_laser_full_cloud_ = true;
}

void LaserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laser_odom_msg) {
  time_laser_odometry_ = laser_odom_msg->header.stamp;

  geometry_msgs::Quaternion geo_quat = laser_odom_msg->pose.pose.orientation;

  transform_sum_.rot.w() = geo_quat.w;
  transform_sum_.rot.x() = geo_quat.x;
  transform_sum_.rot.y() = geo_quat.y;
  transform_sum_.rot.z() = geo_quat.z;

  transform_sum_.pos.x() = float(laser_odom_msg->pose.pose.position.x);
  transform_sum_.pos.y() = float(laser_odom_msg->pose.pose.position.y);
  transform_sum_.pos.z() = float(laser_odom_msg->pose.pose.position.z);

  new_laser_odometry_ = true;
}

void Reset() {
  new_laser_cloud_corner_last_ = false;
  new_laser_cloud_surf_last_ = false;
  new_laser_full_cloud_ = false;
  new_laser_odometry_ = false;
}

bool HasNewData() {
  return new_laser_cloud_corner_last_ && new_laser_cloud_surf_last_ &&
      new_laser_full_cloud_ && new_laser_odometry_ &&
      fabs((time_laser_cloud_corner_last_ - time_laser_odometry_).toSec()) < 0.005 &&
      fabs((time_laser_cloud_surf_last_ - time_laser_odometry_).toSec()) < 0.005 &&
      fabs((time_laser_full_cloud_ - time_laser_odometry_).toSec()) < 0.005;
}

void run() {
  if (!HasNewData()) {
      // waiting for new data to arrive...
      // DLOG(INFO) << "no data received or dropped";
      return;
  }

  Reset();

  PointCloud compact_data;

  PointT compact_point;

  {
    // NOTE: push_back odom
    compact_point.x = transform_sum_.pos.x();
    compact_point.y = transform_sum_.pos.y();
    compact_point.z = transform_sum_.pos.z();
    compact_data.push_back(compact_point);

    compact_point.x = transform_sum_.rot.x();
    compact_point.y = transform_sum_.rot.y();
    compact_point.z = transform_sum_.rot.z();
    compact_point.intensity = transform_sum_.rot.w();
    compact_data.push_back(compact_point);
  }

  {
    compact_point.x = laser_cloud_corner_last_->size();
    compact_point.y = laser_cloud_surf_last_->size();
    compact_point.z = full_cloud_->size();
    compact_data.push_back(compact_point);

    compact_data += (*laser_cloud_corner_last_);
    compact_data += (*laser_cloud_surf_last_);
    compact_data += (*full_cloud_);
  }
  // LOG(INFO) << "Compact data size()" << compact_data.size();
  lio::PublishCloudMsg(pub_compact_data_, compact_data, time_laser_odometry_, "/laser_local");
  // LOG(INFO) << "PUB COMPACT";
}

int main(int argc, char **argv) {

  google::InitGoogleLogging(argv[0]);
  FLAGS_alsologtostderr = true;

  ros::init(argc, argv, "compact_map");

  ros::NodeHandle nh("~");

  sub_laser_cloud_corner_last_ = nh.subscribe<sensor_msgs::PointCloud2>
      ("/laser_cloud_corner_last", 2, LaserCloudCornerLastHandler);

  sub_laser_cloud_surf_last_ = nh.subscribe<sensor_msgs::PointCloud2>
      ("/laser_cloud_surf_last", 2, LaserCloudSurfLastHandler);

  sub_laser_full_cloud_ = nh.subscribe<sensor_msgs::PointCloud2>
      ("/full_odom_cloud", 2, LaserFullCloudHandler);

  sub_laser_odometry_ = nh.subscribe<nav_msgs::Odometry>
      ("/laser_odom_to_init", 2, LaserOdometryHandler);

  pub_compact_data_ = nh.advertise<sensor_msgs::PointCloud2>
      ("/map_compact_data", 2);
  
  ros::Rate r(100);
  while (ros::ok()) {
    run();
    ros::spinOnce();
    r.sleep();
  }

  return 0;
}